| Note: This tutorial assumes that you have completed the previous tutorials: rtmros_nextage/Tutorials/Changing Grippers on Nextage Hardware. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Adding / Changing Robot Model for HiroNxo
Description:Keywords: Kawada, Nextage Open, dual-arm
Tutorial Level: ADVANCED
Next Tutorial: Changing Grippers on Nextage Hardware
Contents
NOTE: All the tutorials available under the URL http://wiki.ros.org/rtmros_nextage/Tutorials are applicable to the multiple products of Kawada Industries; Hiro (only with the one that opensource software is installed) and NEXTAGE OPEN. To simplify the notation in the rest of the tutorials, we use HiroNXO to appoint the aforementioned robots.
Introduction
This wiki tutorial describes how to add or modify robot models for HiroNXO, particularly taking an example of creating a software module (models, program) of a custom hand with Nextage Open. The robot's Digital In and Output (DIO) assignment is available through the Robot Hardware Manual.
Adding a custom hand
Prerequisite
Download Source Code
In this tutorial, we will add your custom hand module into nextage_description, the original package of NEXTAGE OPEN that stores model information. Even if the package is already installed on your machine via binary installation (eg. by using apt-get on Ubuntu), modifying to its protected directory is not a good idea at all and thus you're encouraged to download the source.
$ cd %YOUR_CATKIN_WORKSPACE%/src $ git clone https://github.com/tork-a/rtmros_nextage.git $ cd %YOUR_CATKIN_WORKSPACE%/ $ catkin_make && source devel/setup.bash
If you are not familiar with catkin workspace, create one by referring to tutorial for creating catkin workspace.
We will assume we use nextage_description and nextage_ros_bridge packages from the downloaded source.
Prepare Hand Model
Let's assume that we have a CAD file we'd like to add as a new hand called SuctionHand.wrl, a VRML file that includes a robot model whose coordinate frame is equivalent to original ChunkHand.wrl.
In the factory setting of NEXTAGE OPEN, the initial pose of the arms is like the image above where the upper arm links straight down, links from elbows to hands stretching toward the front of the robot. The absolute coordinate of the robot: x axis points forward from the robot chest, the y axis is to the robot's left, and the z axis is in the vertical up direction. The origin of the hand model is located on the interface flange surface.
Put the .wrl file under nextage_description/models
Convert To Semantic Model for RTM-ROS world
HiroNXO system involves 2 different model formats in addtition to ROS' standard URDF. Hope this diagram helps to understand the relationship between each.
VRML format (extension: .wrl) is used by the underlining controller hrpsys
First, once again check prerequiste, in particular downloading source code on to your machine.
Add your model to OpenHRP3 wrl model file
(TODO: elaborate why we need to do this, what's OpenHRP3 wrl model, etc.)
Change nextage_description/models/main.wrl as follows:
# 'Inline {url "ChunkHand.wrl"}'
'Inline {url "SuctionHand.wrl"}"The portion that contains the same line of code now looks like this:
Transform {
translation -0.065 0 0 children [
# Inline { url "ChuckHand.wrl" }
Inline { url "SuctionHand.wrl" }
]
}
Create COLLADA model
First, make sure you're at the directory where the model files we're editing are.
$ roscd nextage_description/models
Then, run following commands to convert wrl model into COLLADA format:
$ rosrun openhrp3 export-collada -i main.wrl -o main.dae $ cp main.dae ../../nextage_ros_bridge/models/nextage.dae
Create URDF model
You might need to install collada_urdf package in advance by following:
$ apt-get install ros-%ROS_DISTRO%-collada-urdf $ apt-get install ros-hydro-collada-urdf
Run following commands to convert COLLADA model into URDF model:
$ cd ../urdf $ rosrun collada_urdf collada_to_urdf ../models/main.dae --mesh_output_dir=meshes --mesh_prefix=package://nextage_description/urdf/meshes $ sed s/HiroNX/NextageOpen/ HiroNX.urdf > NextageOpen.urdf # NOTE-1
- NOTE-1. if you want to avoid this, you have to add the following change:
DEF HiroNX Humanoid # Current DEF NextageOpen Humanoid # New
This may have several side-effects, for example we have to change args.robot = "RobotHardware0" if args.host else "HiroNX(Robot)0" to NextageOpen(Robot)0 in nextage.py.
Tips
Better to check urdf file with:
$ roslaunch urdf_tutorial display.launch model:=`rospack find nextage_description`/urdf/NextageOpen.urdf gui:=True
You'll see joint_state_publisher operator window as well as RViz.
You might want to change the fixed frame to WAIST on RViz to show the robot model properly. Then use the slider bars on joint_state_publisher to see if certain joints work as expected.
By the way, this does not work with collada file since joint_state_publisher is not capable of reading it.
.conf and .xml files in conf directory
Once done in generating model files, you need to tell the underlining libraries (OpenRTM, OpenHRP3) about the locations of model files. To do so, you just need to build your package so that the configuration files will be generated using the templates that are placed in conf folder in nextage_ros_bridge package. At this time of writing, you might see the following template files:
RobotHardware.conf.in conf.in nosim.RobotHardware.conf.in nosim.conf.in nosim.xml.in xml.in
Now build:
$ cd %YOUR_CATKIN_WORKSPACE% $ catkin_make
This turns those files with .in suffix into files with concrete path values in them, and .in will be removed from the file names.
If you're curious about more detail of these config files, .conf files are for OpenRTM (description, example). xml files are used by OpenHRP3. Also, if there's any question, this ticket might a good place to ask further.
Notify the robot about the new model
Simulator and the real robot can apply the change only by the updated model files being placed at the right location.
Notify the simulator
Replace .wrl file in nextage_description/models with the one you just created.
Apply the change to the robot
Prerequiste for updating model files on QNX
First make sure you have a write access to the following folder:
/opt/jsk/etc/NEXTAGE if you use NEXTAGE and the version of your hrpsys controller inside of QNX is 315.2.8 or higher.
/opt/jsk/etc/HIRONX
if you use NEXTAGE and the version of your hrpsys controller inside of QNX is 315.2.7 or lower.
- if you use Hironx.
Replace models on QNX
Replace .wrl file in the folder your situation corresponds to above, with the one you just created.
Then, reboot QNX.
- After reboot, first verify in the controller level: check the following log files and see if they look the same or similar.
$ cd /opt/jsk/var/log $ more /opt/jsk/var/log/NameServer.log Thu Oct 16 17:55:16 2014: Starting omniNames for the first time. Thu Oct 16 17:55:17 2014: Wrote initial log file. Read log file successfully. Root context is IOR:010000002b00000049444c3a6f6d672e6f72672f436f734e616d696e672f4e616d696e67436f6e746578744578743a312e3000000100 00000000000070000000010102000c00000031302e3235342e32342e31009d3a00000b0000004e616d6553657276696365000300000000000000080000000100 000000545441010000001c000000010000000100010001000000010001050901010001000000090101000354544108000000840640540100c01a Checkpointing Phase 1: Prepare. Checkpointing Phase 2: Commit. Checkpointing completed. $ more /opt/jsk/var/log/ModelLoader.log ready loading /opt/jsk/etc/HIRONX/model/main.wrl Humanoid node Joint nodeWAIST Segment node WAIST_Link Joint nodeCHEST_JOINT0 Segment node CHEST_JOINT0_Link Joint nodeHEAD_JOINT0 Segment node HEAD_JOINT0_Link Joint nodeHEAD_JOINT1 Segment node HEAD_JOINT1_Link VisionSensorCAMERA_HEAD_R VisionSensorCAMERA_HEAD_L Joint nodeRARM_JOINT0 Segment node RARM_JOINT0_Link Joint nodeRARM_JOINT1 Segment node RARM_JOINT1_Link Joint nodeRARM_JOINT2 Segment node RARM_JOINT2_Link Joint nodeRARM_JOINT3 Segment node RARM_JOINT3_Link Joint nodeRARM_JOINT4 Segment node RARM_JOINT4_Link Joint nodeRARM_JOINT5 Segment node RARM_JOINT5_Link ForceSensorrhsensor Joint nodeLARM_JOINT0 Segment node LARM_JOINT0_Link Joint nodeLARM_JOINT1 Segment node LARM_JOINT1_Link Joint nodeLARM_JOINT2 Segment node LARM_JOINT2_Link Joint nodeLARM_JOINT3 Segment node LARM_JOINT3_Link Joint nodeLARM_JOINT4 Segment node LARM_JOINT4_Link Joint nodeLARM_JOINT5 Segment node LARM_JOINT5_Link ForceSensorlhsensor The model was successfully loaded ! $ more /opt/jsk/var/log/rtcd.log Logger::Logger: streambuf address = 0x805fc70 hrpExecutionContext is registered pdgains.file_name: /opt/jsk/etc/HIRONX/hrprtc/PDgains.sav dof = 15 open_iob - shmif instance at 0x80acf58 the number of gyros = 0 the number of accelerometers = 0 the number of force sensors = 0 period = 5[ms], priority = 49 SequencePlayer::onInitialize() period = 5[ms], priority = 49 Then verify in the hrpsys level: (TODO how to do so is in discussion https://github.com/start-jsk/rtmros_hironx/issues/314)
Finally, you can check ROS' tf tree by a graphical tool:
$ rqt &
Then from Plugin menu, open TF Tree that opens rqt_tf_tree. See if the tree makes sense to you.
Create MoveIt! package containing hands
Unless you want to control the hands with MoveIt!, we do not think anything is needed for using MoveIt! with new hands model.
Create Software In Python
You're almost ready to get your hands moving. Now, coding time.
Hand's software implementation of Nextage employs Command Pattern. With this design, task to add a custom hand software module can be minimized; create Command class where the hands' specific functionalities are defined, and bind it to another class that represents hands, and finally use it from "main" robot client module (eg. nextage_client.py).
(Editable original diagram file)
Add Hand's Function By Creating "Command" Class
First, create a python file that extends nextage_ros_bridge/abs_hand_command.py that contains basic stuff for all hand modules, put it in the source folder of nextage_ros_bridge.
$ roscd nextage_ros_bridge/src/nextage_ros_bridge/command $ touch %YOUR_COMMAND%.py
Then from here, we'll look at a code snippet of a concrete example of suction hand python module (cited Feb 2, 2014):
1 #!/usr/bin/env python
2
3 import time
4 import threading
5
6 import rospy
7
8 from abs_hand_command import AbsractHandCommand
9
10
11 class AirhandCommand(AbsractHandCommand):
12 '''dio_writer
13 Following Command design pattern, this class represents command for
14 an Airhand of NEXTAGE OPEN.
15
16 As of 2/1/2014, it's only implemented for a right arm (since there's no
17 testing environment for left arm).
18 '''
19 # TODO: Unittest is needed!!
20
21 # For air hands
22 AIRHAND_DRAWIN = 'drawin'
23 AIRHAND_KEEP = 'keep'
24 AIRHAND_RELEASE = 'release'
25
26 ## Might not be necessary. Maybe use only where you have to specify
27 ## dangerous situation.AIRHAND_KEEP
28 AIRHAND_DANGER = 'danger'
29
30 def __init__(self, hands, hand):
31 '''
32 @see nextage_ros_bridge.command.abs_hand_command.AbsractHandCommand
33 @type hands: nextage_ros_bridge.base_hands.BaseHands
34 @type hand: str
35 @param hand: Side of hand. Variables that are defined in
36 nextage_ros_bridge.base_hands.BaseHands can be used { HAND_L, HAND_R }.
37 '''
38 super(AirhandCommand, self).__init__(hands, hand)
39 self._SLEEP_POST_RELEASE = 3.0
40
41 def execute(self, operation):
42 '''
43 @see abs_hand_command.AbsractHandCommand.execute
44 '''
45 dout = []
46 mask = [self._DIO_SUCTION_R_1, self._DIO_SUCTION_R_2]
47
48 # TODO: Implement for R hand too.
49 if self.AIRHAND_DRAWIN == operation:
50 if self._hands.HAND_L == self._hand:
51 dout = [self._DIO_SUCTION_L_1]
52 elif self._hands.HAND_R == self._hand:
53 dout = [self._DIO_SUCTION_R_1]
54 elif self.AIRHAND_KEEP == operation:
55 if self._hands.HAND_L == self._hand:
56 pass # Do nothing since off for both pins.
57 elif self._hands.HAND_R == self._hand:
58 pass # Do nothing since off for both pins.
59 elif self.AIRHAND_RELEASE == operation:
60 if self._hands.HAND_L == self._hand:
61 dout = [self._DIO_SUCTION_L_2]
62 elif self._hands.HAND_R == self._hand:
63 dout = [self._DIO_SUCTION_R_2]
64
65 # Create a thread to do KEEP action after the specified amount
66 # of time without stopping the program.
67 thread = AirhandReleaseThread(self, self._SLEEP_POST_RELEASE)
68 thread.start()
69 else:
70 # TODO: Might want to thrown exception?
71 rospy.logwarn('No gripper specified. Do nothing.')
72 return
73 self._hands._dio_writer(dout, mask)
74
75 def _assign_dio_names(self):
76 '''
77 @see abs_hand_command.AbsractHandCommand._assign_dio_names
78 '''
79 #DIO reassignment for the class-specific purpose
80 self._DIO_SUCTION_R_1 = self._DIO_22
81 self._DIO_SUCTION_R_2 = self._DIO_23
82 self._DIO_SUCTION_L_1 = self._DIO_27
83 self._DIO_SUCTION_L_2 = self._DIO_28
Now let's look at what this airhand_command.py does step-by-step:
11 class AirhandCommand(AbsractHandCommand):Extending AbsractHandCommand as already mentioned.
Possible commands are defined as member variables. With the design of this particular class, this command represents multiple functionalities that this hand provides (which, may be a little confusing with respect to the concept of Command Pattern). Related to that, it's more useful to keep these accessible to the external classes (ie. no leading underscore with their names).
Initializing with just passing arguments to the constructor of its super class.30 def __init__(self, hands, hand): 31 ''' 32 @see nextage_ros_bridge.command.abs_hand_command.AbsractHandCommand 33 @type hands: nextage_ros_bridge.base_hands.BaseHands 34 @type hand: str 35 @param hand: Side of hand. Variables that are defined in 36 nextage_ros_bridge.base_hands.BaseHands can be used { HAND_L, HAND_R }. 37 ''' 38 super(AirhandCommand, self).__init__(hands, hand)
75 def _assign_dio_names(self): 76 ''' 77 @see abs_hand_command.AbsractHandCommand._assign_dio_names 78 ''' 79 #DIO reassignment for the class-specific purpose 80 self._DIO_SUCTION_R_1 = self._DIO_22 81 self._DIO_SUCTION_R_2 = self._DIO_23 82 self._DIO_SUCTION_L_1 = self._DIO_27 83 self._DIO_SUCTION_L_2 = self._DIO_28
We skip and reach down the bottom of the code first where we re-assign the name of DIOs to adjust to the specific purpose the DIOs are used for this class. To do so by overriding method _assign_dio_names isn't mandatory (though recommended to avoid confusion of other developers or even you in the future).
In this particular class, these four DIOs are used for the stated purposes, which are defined in the hand's electrical design (TODO: reference to electrical design tutorial how to assign DIO if any)). Now going back up the code,
This part is the "body" of the functions your custom hand provides.41 def execute(self, operation):Here you prepare two lists: dout and mask. While writing this hand's code, you're freed from bit manipulation although the interface of the robot's DIO is designed to interact by bit arrays. You just have to specify which the specific bits to use. So the two lists are:
dout: bit values that indicate on/off of the DIO pins.
mask: Represents the enabled pin(s).
Now the operations are coded. Here we're only looking at the first conditional block; the rest is just in similar structure.Simply we assign the bit for the identified side of the hands for the designated operation (draw in in this case).
73 self._hands._dio_writer(dout, mask)This line is required; we send out the bit arrays to the DIO interface.
Bind Hand-command to Hand Class
We've added a hand, a model and its function. So now we're good to use it.
First, instantiate the command class you just created in a Hand class. You can look at a sample here, some of which is cited like:
class Iros13Hands(BaseToolchangerHands):
:
def __init__(self, parent):
'''
@see: AbsractIros13Hands.__init__
'''
self.airhand_l_command = AirhandCommand(self, self.HAND_L)
self.airhand_r_command = AirhandCommand(self, self.HAND_R)Add the side as an argument. That's it for the Hand class.
Very finally, you are all ready to use the hand!
Use Hand-command From Robot Client
From robot's client class, like NextageClient, you can call execute function of each Command class (just like these methods in NextageClient class).
That's it! As I predicted, hand-command is pretty much all the code you need to write in order to add your custom hand module.
Modifying existing 3D models
As explained in the case above, HiroNXO system is based on multiple 3D model formats. This setting comes from the system architecture (ranges over multiple middleware: OpenRTM, OpenRave and ROS). This can be indeed incovenient, and more importantly error-prone especially when adding your custom models / modifying existing models.
For both Hironx and NEXTAGE OPEN robots, the manufacturer provided 3D model in a single format. This single format (collada for Hironx, VRML for NEXTAGE OPEN). To maintain the consistency between model files in different formats, it's the most recommended for you to stick to the originally provided format to add changes (i.e. editing URDF manually is NOT recommended at all). Good thing is that the conversion between model formats is well prepared.
You may also want to consult this informative post for more information about model files relationship for HiroNXO.
For Hironx robot: All 3D models used in Hironx open robot are stored in models folder in hironx_ros_bridge package (Collada) and in models folder in hironx_moveit_config package (URDF), all of which are derived from kawada-hironx.dae collada file.
For NEXTAGE OPEN robot: 3D models are stored in models and urdf folder in nextage_description package. All of which are derived from main.wrl VRML file.
